/* ************************************************************************* **
**    OpenVFIFE - Open System for Vector Form Instrinsic                     **
**                Finite Element Method (VFIFE)                              **
**                GinkGo(Tan Biao)                                           **
**                                                                           **
**                                                                           **
** (C) Copyright 2021, The GinkGo(Tan Biao). All Rights Reserved.            **
**                                                                           **
** Commercial use of this program without express permission of              **
** GinkGo(Tan Biao), is strictly prohibited.  See                            **
** file 'COPYRIGHT'  in main directory for information on usage and          **
** redistribution,  and for a DISCLAIMER OF ALL WARRANTIES.                  **
**                                                                           **
** Developed by:                                                             **
**      Tan Biao (ginkgoltd@outlook.com)                                     **
**                                                                           **
** ************************************************************************* */

// $Date: 2020-05-11 $
// Written: Tan Biao
// Revised:
//
// Purpose: This file contains the class definition for elements, including
// base class BaseElement (abstract base class), StructElement, PlaneElement,
// SolidElement, and several elements derived from StructElement - Link2D,
// Link2DLD, Link3D, Link3DLD, Cable3D, Beam2D and Beam3D.

// The interface:
//

#include "../headers/element.h"
using namespace std;


/*****************************************************************************/
/*                                 helper                                    */
/*****************************************************************************/
inline double euclideanDistance(const StdArray6d &pi, const StdArray6d &pj)
{
    double ans = 0.0;
    for (int i = 0; i < 3; i++)
    {
        ans += (pi[i] - pj[i]) * (pi[i] - pj[i]);
    }
    return sqrt(ans);
};

inline double euclideanDistance(const StdArray6d* pi, const StdArray6d* pj)
{
    double ans = 0.0;
    for (int i = 0; i < 3; i++)
    {
        ans += ((*pi)[i] - (*pj)[i]) * ((*pi)[i] - (*pj)[i]);
    }
    return sqrt(ans);
};

inline void StdArray6dMulNum(StdArray6d &arr, double num)
{
    for (int i = 0; i < 6; i++)
    {
        arr[i] *= num;
    }
}

inline void StdAarrayAddNum(StdArray6d &arr, double num)
{
    for (int i = 0; i < 6; i++)
    {
        arr[i] += num;
    }
}


/*****************************************************************************/
/*                               BaseElement                                 */
/*****************************************************************************/
BaseElement::BaseElement(int id)
{
    id_ = id;
    type_ = "BaseElement";
    num_of_particles_ = 1;
    material_ = NULL;
    section_ = NULL;
    mass_ = 0;
    strain_ = {.sx=0, .sy=0, .sz=0, .sxy=0, .syz=0, .sxz=0};
    stress_ = {.sx=0, .sy=0, .sz=0, .sxy=0, .syz=0, .sxz=0};
    ex_.setZero();
    ey_.setZero();
    ez_.setZero();
    is_failed_ = false;
}

BaseElement::~BaseElement()
{

}

inline bool BaseElement::checkParticles()
{
    if (particles_.size() > num_of_particles_)
    {
        cerr << "Error: Particles' number exceeds the requirement!" << endl;
    }
    return particles_.size() < num_of_particles_;
}

void BaseElement::addParticle(Particle* p)
{
    if (checkParticles())
    {
        if (particles_.count(p->id()))
        {
            cerr << "Warning: Particle" << p->id() << "is already exist, " <<
                 "The new particle will overwrite it!" << endl;
        }
        else
        {
            particles_[p->id()] = p;
        }
    }
}

void BaseElement::setMaterial(BaseMaterial* mat)
{
    material_ = mat;
}

void BaseElement::setSection(BaseSection* sect)
{
    section_ = sect;
}

void BaseElement::info() const
{
    cout << "Element ID: " << id_ << endl;
    cout << "Element Type: " << type_ << endl;
    cout << "Particle ID: (";
    for (auto iter=particles_.begin(); iter != particles_.end(); iter++)
    {
        cout << iter->first << ", ";
    }
    cout << ")" << endl;
    cout << "\nMaterial: " << endl;
    material_->info();
    cout << "\nSection: " << endl;
    section_->info();
    cout << "\n" << endl;
}

int BaseElement::id() const
{
    return id_;
}

string BaseElement::type() const
{
    return type_;
}

int BaseElement::num_of_particles() const
{
    return num_of_particles_;
}

const map<int, Particle*>* BaseElement::particles() const
{
    return &particles_;
}

double BaseElement::mass() const
{
    return mass_;
}

const BaseMaterial* BaseElement::material() const
{
    return material_;
}

const BaseSection* BaseElement::section() const
{
    return section_;
}

const Eigen::Vector3d* BaseElement::ex() const
{
    return &ex_;
}

const Eigen::Vector3d* BaseElement::ey() const
{
    return &ey_;
}

const Eigen::Vector3d* BaseElement::ez() const
{
    return &ez_;
}

const Strain* BaseElement::strain() const
{
    return &strain_;
}

const Stress* BaseElement::stress() const
{
    return &stress_;
}

bool BaseElement::is_failed() const
{
    return is_failed_;
}


/*****************************************************************************/
/*                              StructElement                                */
/*****************************************************************************/
StructElement::StructElement(int id, Particle *pi, Particle*pj,
    const BaseMaterial *mat, const BaseSection * sect):BaseElement(id)
{
    type_ = "StructElement";
    num_of_particles_ = 2;
    pi_ = pi;
    pj_ = pj;
    addParticle(pi);
    addParticle(pj);
    fi_ = {0};
    fj_ = {0};
    material_ = mat;
    section_ = sect;

    // WARNING: the order can't be changed,
    // or elment couldn't be initiated correctly
    calcLength();
    calcMass();
    lt_ = length_;
    la_ = length_;
}

StructElement::~StructElement()
{

}

inline void StructElement::calcLength()
{
    const StdArray6d* ci = pi_->coordinate();
    const StdArray6d* cj = pj_->coordinate();
    length_ = euclideanDistance(ci, cj);
}

inline void StructElement::calcMass()
{
    mass_ = length_ * material_->density() * section_->A();
}

double StructElement::length() const
{
    return length_;
}

double StructElement::calcCriticalTimeStep()
{
    return length_ / material_->calcCe();
}

void StructElement::setParticleForce()
{
    calcElementForce();
    // one particle may be jointed with several element, use addForce()
    pi_->addForce(fi_);
    pj_->addForce(fj_);
}

void StructElement::outputForce(fstream &fout)
{
    fout << id_ << ", ";
    fout << element_force_.fx << ", " << element_force_.sfy << ", ";
    fout << element_force_.sfz << ", " << element_force_.mx << ", ";
    fout << element_force_.my << ", " << element_force_.mz << "\n";
}

void StructElement::outputStress(fstream &fout)
{
    fout << id_ << ", ";
    fout << stress_.sx << ", " << stress_.sy << ", " << stress_.sz << ", ";
    fout << stress_.sxy << ", " << stress_.syz << ", " << stress_.sxz << "\n";
}

void StructElement::outputStrain(fstream &fout)
{
    fout << id_ << ", ";
    fout << strain_.sx << ", " << strain_.sy << ", " << strain_.sz << ", ";
    fout << strain_.sxy << ", " << strain_.syz << ", " << strain_.sxz << "\n";
}


/*****************************************************************************/
/*                               PlaneElement                                */
/*****************************************************************************/
PlaneElement::PlaneElement(int id, vector<Particle*> ps, BaseMaterial* mat,
    BaseSection* sect): BaseElement(id)
{
    type_ = "PlaneElement";
    for (auto iter = ps.begin(); iter != ps.end(); iter++)
    {
        addParticle(*iter);
    }
    material_ = mat;
    section_ = sect;
    // TODO:
}

PlaneElement::~PlaneElement()
{

}


/*****************************************************************************/
/*                               SolidElement                                */
/*****************************************************************************/
SolidElement::SolidElement(int id, vector<Particle*> ps,
    const BaseMaterial* mat, const BaseSection* sect): BaseElement(id)
{
    type_ = "SolidElement";
    for (auto iter = ps.begin(); iter != ps.end(); iter++)
    {
        addParticle(*iter);
    }
    material_ = mat;
    section_ = sect;
    // TODO:
}

SolidElement::~SolidElement()
{

}


/*****************************************************************************/
/*                                   Link2D                                  */
/*****************************************************************************/
Link2D::Link2D(int id, Particle *pi, Particle *pj, const BaseMaterial *mat,
    const BaseSection *sect):StructElement(id, pi, pj, mat, sect)
{
    type_ = "Link2D";
    posi_ = pi_->current_position();
    posj_ = pj_->current_position();

    calcOrientVector();
    setParticleDof();
    setParticleMass();
}

Link2D::~Link2D()
{

}

void Link2D::setParticleDof()
{
    pi_->activateDof("Ux");
    pi_->activateDof("Uy");
    pj_->activateDof("Ux");
    pj_->activateDof("Uy");
}

void Link2D::setParticleMass()
{
    StdArray6d mass {mass_/2.0, mass_/2.0, 0, 0, 0, 0};
    pi_->addLumpedMass(mass);
    pj_->addLumpedMass(mass);
}

void Link2D::calcOrientVector()
{
    const StdArray6d* ci = pi_->coordinate();
    const StdArray6d* cj = pj_->coordinate();
    ex_(0) = ((*cj)[0] - (*ci)[0]) / length_;
    ex_(1) = ((*cj)[1] - (*ci)[1]) / length_;
}

void Link2D::calcElementForce()
{
    calcOrientVector();
    lt_ = euclideanDistance(posi_, posj_);
    strain_.sx = (lt_ - la_) / length_;
    stress_.sx = material_->E() * strain_.sx;
    element_force_.fx = stress_.sx * section_->A();

    // fill fi_ and fj_
    fi_.fill(0);
    fj_.fill(0);
    fi_[0] = element_force_.fx * ex_(0);
    fi_[1] = element_force_.fx * ex_(1);
    fj_[0] = -element_force_.fx * ex_(0);
    fj_[1] = -element_force_.fx * ex_(1);
}

void Link2D::outputForce(fstream &fout)
{
    fout << id_ << ", " << element_force_.fx << "\n";
}

inline void printStdArray6d(const StdArray6d* arr)
{
    for (int i = 0; i < 6; i++)
    {
        cout << (*arr)[i] << ", ";
    }
    cout << endl;
}

inline void printStdArray6d(const StdArray6d &arr)
{
    for (int i = 0; i < 6; i++)
    {
        cout << arr[i] << ", ";
    }
    cout << endl;
}


/*****************************************************************************/
/*                                   Link2DLD                                */
/*****************************************************************************/
Link2DLD::Link2DLD(int id, Particle* pi, Particle* pj, const BaseMaterial* mat,
    const BaseSection* sect): Link2D(id, pi, pj, mat, sect)
{
    type_ = "Link2DLD";
    mstress_ = 0;
    mstrain_ = 0;
    mps_ = 0;
    malpha_ = 0;
    mq_ = 0;
}

Link2DLD::~Link2DLD()
{

}

void Link2DLD::calcOrientVector()
{
    la_ = lt_;
    lt_ = euclideanDistance(posi_, posj_);
    ex_(0) = ((*posj_)[0] - (*posi_)[0]) / lt_;
    ex_(1) = ((*posj_)[1] - (*posi_)[1]) / lt_;
}

void Link2DLD::calcElementForce()
{
    // fill fi_ and fj_
    fi_.fill(0);
    fj_.fill(0);

    // element still working
    if (!is_failed_)
    {
        calcOrientVector();                         // update direction vector
        double delta_strain = (lt_ - la_) / la_;    // increment of strain
        if (material_->isFractured(mstrain_ + delta_strain))
        {
            // material fractured
            is_failed_ = true;
        }
        else
        {
            // get tagent modulus and update stress and strain status
            double eq = material_->Eq(delta_strain, &mstress_, &mstrain_,
                                      &mps_, &malpha_, &mq_);
            strain_.sx = mstrain_;
            stress_.sx = mstress_;
            element_force_.fx = mstress_ * section_->A();

            // deprecated, same function as previous 3 expressions
            // double delta_stress = eq * delta_strain;
            // double delta_force = delta_stress * section_->A();
            // strain_.sx += delta_strain;
            // stress_.sx += delta_stress;
            // element_force_.fx += delta_force;

            // fill fi_ and fj_
            fi_[0] = element_force_.fx * ex_(0);
            fi_[1] = element_force_.fx * ex_(1);
            fj_[0] = -element_force_.fx * ex_(0);
            fj_[1] = -element_force_.fx * ex_(1);
        }
    }
}


/*****************************************************************************/
/*                                   Link3D                                  */
/*****************************************************************************/
Link3D::Link3D(int id, Particle *pi, Particle *pj, const BaseMaterial *mat,
    const BaseSection *sect): StructElement(id, pi, pj, mat, sect)
{
    type_ = "Link3D";
    posi_ = pi_->current_position();
    posj_ = pj_->current_position();

    calcOrientVector();
    setParticleDof();
    setParticleMass();
}

Link3D::~Link3D()
{

}

void Link3D::setParticleDof()
{
    pi_->activateDof("Ux");
    pi_->activateDof("Uy");
    pi_->activateDof("Uz");
    pj_->activateDof("Ux");
    pj_->activateDof("Uy");
    pj_->activateDof("Uz");
}

void Link3D::setParticleMass()
{
    StdArray6d mass {mass_/2.0, mass_/2.0, mass_/2.0, 0, 0, 0};
    pi_->addLumpedMass(mass);
    pj_->addLumpedMass(mass);
}

void Link3D::calcOrientVector()
{
    const StdArray6d* ci = pi_->coordinate();
    const StdArray6d* cj = pj_->coordinate();
    ex_(0) = ((*cj)[0] - (*ci)[0]) / length_;
    ex_(1) = ((*cj)[1] - (*ci)[1]) / length_;
    ex_(2) = ((*cj)[2] - (*ci)[2]) / length_;
}

void Link3D::calcElementForce()
{
    calcOrientVector();
    lt_ = euclideanDistance(posi_, posj_);
    strain_.sx = (lt_ - la_) / length_;
    stress_.sx = material_->E() * strain_.sx;
    element_force_.fx = stress_.sx * section_->A();

    // fill fi_ and fj_
    fi_.fill(0);
    fj_.fill(0);
    fi_[0] = element_force_.fx * ex_(0);
    fi_[1] = element_force_.fx * ex_(1);
    fi_[2] = element_force_.fx * ex_(2);
    fj_[0] = -element_force_.fx * ex_(0);
    fj_[1] = -element_force_.fx * ex_(1);
    fj_[2] = -element_force_.fx * ex_(2);
}

void Link3D::outputForce(fstream &fout)
{
    fout << id_ << ", " << element_force_.fx << "\n";
}


/*****************************************************************************/
/*                                   Link3DLD                                */
/*****************************************************************************/
Link3DLD::Link3DLD(int id, Particle *pi, Particle *pj, const BaseMaterial *mat,
    const BaseSection *sect):Link3D(id, pi, pj, mat, sect)
{
    type_ = "Link3DLD";
    mstress_ = 0;
    mstrain_ = 0;
    mps_ = 0;
    malpha_ = 0;
    mq_ = 0;
}

Link3DLD::~Link3DLD()
{

}

void Link3DLD::calcOrientVector()
{
    la_ = lt_;
    lt_ = euclideanDistance(posi_, posj_);
    ex_(0) = ((*posj_)[0] - (*posi_)[0]) / lt_;
    ex_(1) = ((*posj_)[1] - (*posi_)[1]) / lt_;
    ex_(2) = ((*posj_)[2] - (*posi_)[2]) / lt_;

}

void Link3DLD::calcElementForce()
{
    // fill fi_ and fj_ with 0,
    fi_.fill(0);
    fj_.fill(0);

    if (!is_failed_)    // element still working
    {
        calcOrientVector();                         // update direction vector
        double delta_strain = (lt_ - la_) / la_;    // increment of strain
        if (material_->isFractured(mstrain_ + delta_strain))
        {
            // material fractured
            is_failed_ = true;
            cout << "Element " << id_ << " is failed." << endl;
        }
        else
        {
            // get tangent modulus and upadte stress, strain status
            double eq = material_->Eq(delta_strain, &mstress_, &mstrain_,
                                      &mps_, &malpha_, &mq_);
            strain_.sx = mstrain_;
            stress_.sx = mstress_;
            element_force_.fx = mstress_ * section_->A();

            // deprecated, same function as previous 3 expressions
            // double delta_stress = eq * delta_strain;
            // double delta_force = delta_stress * section_->A();
            // strain_.sx += delta_strain;
            // stress_.sx += delta_stress;
            // element_force_.fx += delta_force;

            // fill fi_ and fj_
            fi_[0] = element_force_.fx * ex_(0);
            fi_[1] = element_force_.fx * ex_(1);
            fi_[2] = element_force_.fx * ex_(2);
            fj_[0] = -element_force_.fx * ex_(0);
            fj_[1] = -element_force_.fx * ex_(1);
            fj_[2] = -element_force_.fx * ex_(2);
        }
    }
}


/*****************************************************************************/
/*                                  Cable3D                                  */
/*****************************************************************************/
Cable3D::Cable3D(int id, Particle* pi, Particle* pj, const BaseMaterial* mat,
    const BaseSection* sect, double prestrain): Link3DLD(id, pi, pj, mat, sect)
{
    type_ = "Cable3D";
    if (mat->type() != "LinearElastic")
    {
        cout << "Warning: The Cable3D element only support LinerElastic ";
        cout << " material.\n The inputed material type is ";
        cout << mat->type() << ". It might not work!" << endl;
    }
    if (prestrain < 0.0)
    {
        cout << "Warning: The prestrain of Cable3D element must be tensile";
        cout <<  "strain! The inputed prestrain will be ignored, ";
        cout << "and the prestrain will be set as 0.0." << endl;
        prestrain_ = 0.0;
    }
    else
    {
        prestrain_ = prestrain;
    }
    // setting prestress
    strain_.sx = prestrain_;
    stress_.sx = prestrain_ * mat->E();
    element_force_.fx = stress_.sx * sect->A();
}

Cable3D::~Cable3D()
{

};

void Cable3D::calcElementForce()
{
    calcOrientVector();
    double delta_strain = (lt_ - la_) / la_;    // RETHINK: is it reasonable?

    // check the element status
    if (strain_.sx + delta_strain < 0.0)
    {
        // cable relaxation
        strain_.sx = 0.0;
        stress_.sx = 0.0;
        element_force_.fx = 0.0;
    }
    else
    {
        // cable working
        double delta_stress = material_->E() * delta_strain;
        double delta_force = delta_stress * section_->A();
        strain_.sx += delta_strain;
        stress_.sx += delta_stress;
        element_force_.fx += delta_force;
    }

    // fill fi_ and fj_
    fi_.fill(0);
    fj_.fill(0);
    fi_[0] = element_force_.fx * ex_(0);
    fi_[1] = element_force_.fx * ex_(1);
    fi_[2] = element_force_.fx * ex_(2);
    fj_[0] = -element_force_.fx * ex_(0);
    fj_[1] = -element_force_.fx * ex_(1);
    fj_[2] = -element_force_.fx * ex_(2);
};


/*****************************************************************************/
/*                                   Beam2D                                  */
/*****************************************************************************/
Beam2D::Beam2D(int id, Particle *pi, Particle *pj, const BaseMaterial *mat,
    const BaseSection *sect):StructElement(id, pi, pj, mat, sect)
{
    type_ = "Beam2D";
    // initiate position pointer
    curr_posi_ = pi->current_position();
    curr_posj_ = pj->current_position();
    prev_posi_ = pi->previous_position();
    prev_posj_ = pj->previous_position();

    // initiate element force
    emfi_ = {.fx=0, .sfy=0, .sfz=0, .mx=0, .my=0, .mz=0};
    emfj_ = {.fx=0, .sfy=0, .sfz=0, .mx=0, .my=0, .mz=0};

    // initiate previous direction vector
    exa_.setZero();
    eya_.setZero();
    eza_.setZero();

    // initiate deformation of beam, [axial, thetai, thetaj]
    elem_rotation_angle_ = 0;

    // initiate stiffness matrix
    double A, I;
    A = sect->A();
    I = sect->Izz();
    stiffness_ << A, 0, 0,
                  0, 4*I, 2*I,
                  0, 2*I, 4*I;

    calcOrientVector();
    setParticleDof();
    setParticleMass();
}

Beam2D::~Beam2D()
{

}

double Beam2D::calcCriticalTimeStep()
{
    double t = 0;
    double t1 = StructElement::calcCriticalTimeStep();
    double c = material_->calcCe();
    double I = section_->Izz();
    double A = section_->A();
    double tmp = sqrt(3 * I * (3.0 / (12 * I + A * length_ * length_)
                               + 1.0 / A / length_ / length_ ));
    double t2 = 0.5 * length_ / c / tmp;
    if (t1 < t2)
    {
        t = t1;
    }
    else
    {
        t = t2;
    }
    return t;
}

void Beam2D::setParticleDof()
{
    pi_->activateDof("Ux");
    pi_->activateDof("Uy");
    pi_->activateDof("Rotz");
    pj_->activateDof("Ux");
    pj_->activateDof("Uy");
    pj_->activateDof("Rotz");
}

void Beam2D::setParticleMass()
{
    // for element coordinates the ix = sqrt(Izz / A); however in section
    // coordinates the Iyy means Izz of element coordiantes
    double ix2 = section_->Izz() / section_->A();
    StdArray6d mass {mass_/2.0, mass_/2.0, 0, 0, 0, mass_*ix2/2.0};
    pi_->addLumpedMass(mass);
    pj_->addLumpedMass(mass);
}

void Beam2D::calcOrientVector()
{
    lt_ = euclideanDistance(curr_posi_, curr_posj_);
    la_ = euclideanDistance(prev_posi_, prev_posj_);
    for (int i = 0; i < 3; i++)
    {
        ex_(i) = ((*curr_posj_)[i] - (*curr_posi_)[i]) / lt_;
        exa_(i) = ((*prev_posj_)[i] - (*prev_posi_)[i]) / la_;
    }
}

void Beam2D::calcElementForce()
{
    /*
    Internal force of Beam Element includes deformation anlysis and internal
    force calculation, and can be divided into the following step;
        1) virtual move to reference configuration (last time step)
        2) deformation calculation
        3) internal force calculation
        4) move back to current time step
    */

    calcOrientVector();
    // element deformation
    elem_rotation_angle_ = asin(exa_(0) * ex_(1) - exa_(1) * ex_(0));

    Eigen::Vector3d deform;
    deform(0) = (lt_ - la_);
    deform(1) = (*curr_posi_)[5] - (*prev_posi_)[5] - elem_rotation_angle_;
    deform(2) = (*curr_posj_)[5] - (*prev_posj_)[5] - elem_rotation_angle_;

    // internal force
    double delta_strain = deform(0) / la_;
    double E = material_->E();
    force_ += E * stiffness_ * deform / la_;

    // element coordinate
    Eigen::Vector3d fi, fj;
    fi(0) = -force_(0);
    fi(1) = (force_(1) + force_(2)) / la_;
    fi(2) = force_(1);
    fj(0) = force_(0);
    fj(1) = -fi(1);
    fj(2) = force_(2);

    // transfer to global coordinate
    Eigen::Matrix3d Q;
    Q << exa_(0), exa_(1), 0,
         -exa_(1), exa_(0), 0,
         0, 0, 1;
    fi = Q.transpose() * fi;
    fj = Q.transpose() * fj;

    // Forward motion
    Eigen::Matrix3d R;
    R << cos(elem_rotation_angle_), sin(elem_rotation_angle_), 0,
         -sin(elem_rotation_angle_), cos(elem_rotation_angle_), 0,
         0, 0, 1;
    fi = R.transpose() * fi;
    fj = R.transpose() * fj;

    // record element force, FIXME: it's based on Golabal Coordiante System
    emfi_.fx = fi(0);
    emfi_.sfy = fi(1);
    emfi_.mz = fi(2);
    emfj_.fx = fj(0);
    emfj_.sfy = fj(1);
    emfj_.mz = fj(2);

    // fill fi_ and fj_
    fi_.fill(0);
    fj_.fill(0);
    fi_[0] = -fi(0);
    fi_[1] = -fi(1);
    fi_[5] = -fi(2);
    fj_[0] = -fj(0);
    fj_[1] = -fj(1);
    fj_[5] = -fj(2);
}

void Beam2D::outputForce(fstream &fout)
{
    fout << id_ << ", " << pi_->id() << ", ";
    fout << emfi_.fx << ", " << emfi_.sfy << ", ";
    fout << emfi_.sfz << ", " << emfi_.mx << ", ";
    fout << emfi_.my << ", " << emfi_.mz << "\n";

    fout << id_ << ", " << pj_->id() << ", ";
    fout << emfj_.fx << ", " << emfj_.sfy << ", ";
    fout << emfj_.sfz << ", " << emfj_.mx << ", ";
    fout << emfj_.my << ", " << emfj_.mz << "\n";
}

void Beam2D::outputStress(fstream &fout)
{
    cout << "Warning: Not Implement!" << endl;
}

void Beam2D::outputStrain(fstream &fout)
{
    cout << "Warning: Not Implement!" << endl;
}


/*****************************************************************************/
/*                                   Beam3D                                  */
/*****************************************************************************/
inline void Beam3D::initiateBeam3D()
{
    type_ = "Beam3D";
    // initiate position pointer
    curr_posi_ = pi_->current_position();
    curr_posj_ = pj_->current_position();
    prev_posi_ = pi_->previous_position();
    prev_posj_ = pj_->previous_position();

    // initiate previous direction vector
    exa_.setZero();
    eya_.setZero();
    eza_.setZero();

    // others
    omega_.setZero();
    gamma_.setZero();
    r_gamma_.setIdentity();
    ef1h_.setZero();
    em1h_.setZero();
    ef2h_.setZero();
    em2h_.setZero();

    // initiate element force
    emfi_ = {.fx=0, .sfy=0, .sfz=0, .mx=0, .my=0, .mz=0};
    emfj_ = {.fx=0, .sfy=0, .sfz=0, .mx=0, .my=0, .mz=0};
}

Beam3D::Beam3D(int id, Particle* pi, Particle* pj, const BaseMaterial* mat,
    const BaseSection* sect):StructElement(id, pi, pj, mat, sect)
{
    // initiate reference node coordinate
    reference_node_ = {0, 0, 1, 0, 0, 0};
    initiateBeam3D();
    initiatePrincipleCoord();
    calcOrientVector();
    setParticleDof();
    setParticleMass();
}

Beam3D::Beam3D(int id, Particle* pi, Particle* pj, const BaseMaterial* mat,
    const BaseSection* sect, const StdArray3d ref_node):
    StructElement(id, pi, pj, mat, sect)
{
    // initiate reference node coordinate
    reference_node_ = {ref_node[0], ref_node[1], ref_node[2], 0, 0, 0};
    initiateBeam3D();
    initiatePrincipleCoord();
    calcOrientVector();
    setParticleDof();
    setParticleMass();
}

Beam3D::~Beam3D()
{

}

double Beam3D::calcCriticalTimeStep()
{
    double t = 0;
    double t1 = StructElement::calcCriticalTimeStep();
    double c = material_->calcCe();
    double Iyy = section_->Iyy();
    double Izz = section_->Izz();
    double I = 0;
    if (Iyy < Izz)
    {
        I = Izz;
    }
    else
    {
        I = Iyy;
    }
    double A = section_->A();
    double tmp = sqrt(3 * I * (3.0 / (12 * I + A * length_ * length_)
                               + 1.0 / A / length_ / length_ ));
    double t2 = 0.5 * length_ / c / tmp;
    if (t1 < t2)
    {
        t = t1;
    }
    else
    {
        t = t2;
    }
    return t;
}

void Beam3D::setParticleDof()
{
    vector<string> dofs = {"Ux", "Uy", "Uz", "Rotx", "Roty", "Rotz"};
    for (int i = 0; i < 6; i++)
    {
        pi_->activateDof(dofs[i]);
        pj_->activateDof(dofs[i]);
    }
}

void Beam3D::setParticleMass()
{
    // for element coordinates the ix = sqrt(Izz / A); however in section
    // coordinates the Iyy means Izz of element coordiantes
    double iy2 = section_->Iyy() / section_->A();
    double iz2 = section_->Izz() / section_->A();
    double ix2 = iy2 + iz2;
    double m = mass_/2.0;
    StdArray6d mass {m, m, m, ix2 * m, iy2 * m, iz2 * m};
    pi_->addLumpedMass(mass);
    pj_->addLumpedMass(mass);

    Eigen::Matrix3d Im = Eigen::Matrix3d::Zero();
    Im(0, 0) = m * ix2;
    Im(1, 1) = m * iy2;
    Im(2, 2) = m * iz2;
    Im = omega_.transpose() * Im * omega_;
    pi_->addInertiaMass(Im);
    pj_->addInertiaMass(Im);
}

void Beam3D::calcOrientVector()
{
    lt_ = euclideanDistance(curr_posi_, curr_posj_);
    la_ = euclideanDistance(prev_posi_, prev_posj_);
    for (int i = 0; i < 3; i++)
    {
        ex_(i) = ((*curr_posj_)[i] - (*curr_posi_)[i]) / lt_;
        exa_(i) = ((*prev_posj_)[i] - (*prev_posi_)[i]) / la_;
    }
}

void Beam3D::initiatePrincipleCoord()
{
    const StdArray6d* ci = pi_->coordinate();
    const StdArray6d* cj = pj_->coordinate();

    // step 1: calculate ey_;
    Eigen::RowVector3d exz;
    double tmp = euclideanDistance(&reference_node_, ci);
    for (int i = 0; i < 3; i++)
    {
        ex_(i) = ((*cj)[i] - (*ci)[i]) / length_;
        exz(i) = (reference_node_[i] - (*ci)[i]) / tmp;
    }
    ey_ = exz.cross(ex_);

    // if ex_ // z then ey_ = {0, 1, 0}
    Eigen::RowVector3d z;
    z << 0, 0, 1;
    if (acos(ex_.dot(z) / ex_.norm())< 1e-4 * length_)
    {
        ey_ << 0, 1, 0;
    }
    else
    {
        ey_ /= ey_.norm();
    }

    // step 2: calculate ez_ and integrate omega_
    ez_ = ex_.cross(ey_);
    omega_ << ex_, ey_, ez_;
    omega_.transposeInPlace();
}

void Beam3D::rotatePrincipleAxial()
{
    calcOrientVector();
    // step 1: rotation of cross section
    Eigen::Vector3d gamma_b1, beta_b1;
    for (int i = 3; i < 6; i++)
    {
        beta_b1(i-3) = (*curr_posi_)[i] - (*prev_posi_)[i];
    }
    gamma_b1 = beta_b1.dot(exa_) * exa_;

    // setp 2: rotation of principle axial
    Eigen::Vector3d eba = exa_.cross(ex_);
    double eba_norm = eba.norm();
    double principle_rotation = 0;
    if (eba_norm > 1e-16)
    {
        principle_rotation = asin(eba_norm);
        eba /= eba_norm;
    }
    else
    {
        principle_rotation = 0;
        eba.setZero();
    }
    gamma_ = gamma_b1 + principle_rotation * eba;

    // step 3: normalization of gamma
    double gamma_norm = gamma_.norm();
    Eigen::Vector3d e_gamma;
    if (gamma_norm > 1e-16)
    {
        e_gamma = gamma_ / gamma_norm;
    }
    else
    {
        e_gamma.setZero();
    }

    // step 4: calculate transfer matrix: time a -> t
    Eigen::Matrix3d tmp;
    tmp << 0, -e_gamma(2), e_gamma(1),
           e_gamma(2), 0, -e_gamma(0),
           -e_gamma(1), e_gamma(0), 0;
    r_gamma_.setIdentity();
    r_gamma_ += sin(gamma_norm) * tmp + (1 - cos(gamma_norm)) * tmp * tmp;
}

void Beam3D::calcElementForce()
{
    // calcOrientVector();
    exa_ = omega_.row(0);
    eya_ = omega_.row(1);
    eza_ = omega_.row(2);

    // element displace vector
    Eigen::Matrix<double, 6, 1> uvwi, uvwj;

    for (int i = 0; i < 6; i++)
    {
        uvwi(i) = (*curr_posi_)[i] - (*prev_posi_)[i];
        uvwj(i) = (*curr_posj_)[i] - (*prev_posj_)[i];
    }

    // step 1: principle axial rotation, change r_gamma_
    rotatePrincipleAxial();

    // step 2: convert particle rotation to element coordinate system and
    //         virtual reverse motion
    Eigen::Vector3d phi1h, phi2h, gamma_t;
    gamma_t << gamma_.dot(exa_), gamma_.dot(eya_), gamma_.dot(eza_);
    phi1h = omega_ * uvwi.tail<3>() - gamma_t;
    phi2h = omega_ * uvwj.tail<3>() - gamma_t;

    // step 3: element forces & moment increasement in principal coordinate
    //         system at time t, but interpret at time ta
    double delta_strain = (lt_ - la_) / la_;
    double E = material_->E();
    double G = material_->G();
    double A = section_->A();
    double Iy = section_->Iyy();
    double Iz = section_->Izz();
    double Ix = Iy + Iz;

    ef2h_(0) += E * A * delta_strain;
    em2h_(0) += G * Ix * phi2h(0) / la_;
    em1h_(1) += E * Iy * (4.0 * phi1h(1) + 2.0 * phi2h(1)) / la_;
    em2h_(1) += E * Iy * (4.0 * phi2h(1) + 2.0 * phi1h(1)) / la_;
    em1h_(2) += E * Iz * (4.0 * phi1h(2) + 2.0 * phi2h(2)) / la_;
    em2h_(2) += E * Iz * (4.0 * phi2h(2) + 2.0 * phi1h(2)) / la_;

    // Element equilibrium
    ef1h_(0) = -ef2h_(0);
    em1h_(0) = -em2h_(0);
    ef2h_(1) = -(em1h_(2) + em2h_(2)) / la_;
    ef2h_(2) =  (em1h_(1) + em2h_(1)) / la_;
    ef1h_(1) = -ef2h_(1);
    ef1h_(2) = -ef2h_(2);

   // step 4: Element forces & moment in global coordinate and forward motion
    Eigen::Vector3d ef1, ef2, em1, em2;
    ef1 = r_gamma_ * omega_.transpose() * ef1h_;
    ef2 = r_gamma_ * omega_.transpose() * ef2h_;
    em1 = r_gamma_ * omega_.transpose() * em1h_;
    em2 = r_gamma_ * omega_.transpose() * em2h_;

    // step 5: update coordinate transfer matrix
    ey_ = r_gamma_ * eya_;
    ez_ = ex_.cross(ey_);
    omega_ << ex_, ey_, ez_;
    omega_.transposeInPlace();

    // step 6: set element internal force, FIXME: may not right
    ef1h_ = omega_ * ef1;
    em1h_ = omega_ * em1;
    ef2h_ = omega_ * ef2;
    em2h_ = omega_ * em2;
    emfi_ = {.fx=ef1h_(0), .sfy=ef1h_(1), .sfz=ef1h_(2),
             .mx=em1h_(0), .my=em1h_(1), .mz=em1h_(2)};
    emfj_ = {.fx=ef2h_(0), .sfy=ef2h_(1), .sfz=ef2h_(2),
             .mx=em2h_(0), .my=em2h_(1), .mz=em2h_(2)};

    // fill fi_ and fj_
    fi_.fill(0);
    fj_.fill(0);
    for (int i = 0; i < 3; i++)
    {
        fi_[i] = -ef1(i);
        fj_[i] = -ef2(i);
    }
    for (int i = 0; i < 3; i++)
    {
        fi_[i+3] = -em1(i);
        fj_[i+3] = -em2(i);
    }
}

void Beam3D::outputForce(fstream &fout)
{
    fout << id_ << ", " << pi_->id() << ", ";
    fout << emfi_.fx << ", " << emfi_.sfy << ", ";
    fout << emfi_.sfz << ", " << emfi_.mx << ", ";
    fout << emfi_.my << ", " << emfi_.mz << "\n";

    fout << id_ << ", " << pj_->id() << ", ";
    fout << emfj_.fx << ", " << emfj_.sfy << ", ";
    fout << emfj_.sfz << ", " << emfj_.mx << ", ";
    fout << emfj_.my << ", " << emfj_.mz << "\n";
}

void Beam3D::outputStress(fstream &fout)
{
    // TODO:
    cout << "Warning: Not Implement!" << endl;

}

void Beam3D::outputStrain(fstream &fout)
{
    // TODO:
    cout << "Warning: Not Implement!" << endl;
}


/*****************************************************************************/
/*                               PlasticBeam2D                               */
/*****************************************************************************/
PlasticBeam2D::PlasticBeam2D(int id, Particle* pi, Particle*pj,
    const BaseMaterial* mat, const BaseSection* sect,
    const SectionGridParameter &para):
    Beam2D(id, pi, pj, mat, sect), lobatto(para.sections)
{
    type_ = "PlasticBeam2D";
    if (para.sections <= 0)
    {
        cerr << "Warning: integral points must be positive." << endl;
    }
    integrals_points_ = para.sections;

    // create GridSection and initaite response containers
    grid_ = CreateGrid::getInstance(sect, para);
    for (int i = 0; i < para.sections; i++)
    {
        SectionResponse* s = new SectionResponse();
        grid_->initiateResponseContainers(sect, s);
        s->setCellMaterial(mat);
        response_.push_back(s);
    }
}

PlasticBeam2D::~PlasticBeam2D()
{
    // release reponse_ container
    for (int i = 0; i < response_.size(); i++)
    {
        delete response_[i];
        response_[i] = nullptr;
    }
    response_.clear();
    response_.shrink_to_fit();
}

inline void PlasticBeam2D::isFailed()
{
    for (int i = 0; i < response_.size(); i++)
    {
        // degree of damage of Section reaches 1.0
        if (fabs(response_[i]->damageDegree() - 1) < 1e-6)
        {
            is_failed_ = true;
        }
    }
}

void PlasticBeam2D::calcElementForce()
{
    /*
    Internal force of Beam Element includes deformation anlysis and internal
    force calculation, and can be divided into the following step;
        1) virtual move to reference configuration (last time step)
        2) deformation calculation
        3) internal force calculation, section force, element force
        4) move back to current time step
    */
    calcOrientVector();
    // element deformation
    elem_rotation_angle_ = asin(exa_(0) * ex_(1) - exa_(1) * ex_(0));

    Eigen::VectorXd deform(5);
    deform(0) = (lt_ - la_);
    deform(1) = 0.0;
    deform(2) = (*curr_posi_)[5] - (*prev_posi_)[5] - elem_rotation_angle_;
    deform(3) = 0.0;
    deform(4) = (*curr_posj_)[5] - (*prev_posj_)[5] - elem_rotation_angle_;

    // internal force
    // integration points position
    for (int i = 0; i < integrals_points_; i++)
    {
        double x = lobatto.x(i) * la_ / 2.0 + la_ / 2.0;

        // section internal force, Gauss-Legendre integration
        // tranfer matrix: node displacement --> section strain
        Eigen::RowVectorXd transfer(5);
        double s = x / la_;
        transfer << 1, 6 * s - 4, 4 - 6 * s, 6 * s - 2, 2 - 6 * s;
        response_[i]->response(transfer, deform);
    }

    // Element internal force, Gauss-Lobatto integration
    vector<double> force;
    for (int i = 0; i < 5; i++)
    {
        vector<double> tmp;
        for (int j = 0; j < integrals_points_; j++)
        {
            // cout << "force:" << response_[j]->force[i] << endl;
            tmp.push_back(response_[j]->force[i]);
        }
        double res = lobatto.integrate1d(tmp, 0, la_);
        force.push_back(res);
    }

    // Element internal force, Gauss-Lobatto integration
    force_(0) = force[0];
    force_(1) = force[2];
    force_(2) = force[4];

    // element coordinate
    Eigen::Vector3d fi, fj;
    fi(0) = -force_(0);
    fi(1) = (force_(1) + force_(2)) / la_;
    fi(2) = force_(1);
    fj(0) = force_(0);
    fj(1) = -fi(1);
    fj(2) = force_(2);

    // transfer to global coordinate
    Eigen::Matrix3d Q;
    Q << exa_(0), exa_(1), 0,
         -exa_(1), exa_(0), 0,
         0, 0, 1;
    fi = Q.transpose() * fi;
    fj = Q.transpose() * fj;

    // Forward motion
    Eigen::Matrix3d R;
    R << cos(elem_rotation_angle_), sin(elem_rotation_angle_), 0,
         -sin(elem_rotation_angle_), cos(elem_rotation_angle_), 0,
         0, 0, 1;
    fi = R.transpose() * fi;
    fj = R.transpose() * fj;

    // record element force, FIXME: may not right
    emfi_.fx = fi(0);
    emfi_.sfy = fi(1);
    emfi_.mz = fi(2);
    emfj_.fx = fj(0);
    emfj_.sfy = fj(1);
    emfj_.mz = fj(2);

    // fill fi_ and fj_
    fi_.fill(0);
    fj_.fill(0);
    fi_[0] = -fi(0);
    fi_[1] = -fi(1);
    fi_[5] = -fi(2);
    fj_[0] = -fj(0);
    fj_[1] = -fj(1);
    fj_[5] = -fj(2);

    // check if the element is broken
    isFailed();
}


/*****************************************************************************/
/*                                PlasticBeam3D                              */
/*****************************************************************************/
inline void PlasticBeam3D::initiatePlasticBeam3D(const BaseMaterial* mat,
    const BaseSection* sect, const SectionGridParameter &para)
{
    // initiate reference node coordinate
    type_ = "PlasticBeam3D";
    if (para.sections <= 0)
    {
        cerr << "Warning: integral points must be positive." << endl;
    }
    integrals_points_ = para.sections;

    // create GridSection and initaite response containers
    grid_ = CreateGrid::getInstance(sect, para);
    for (int i = 0; i < para.sections; i++)
    {
        SectionResponse* s = new SectionResponse();
        grid_->initiateResponseContainers(sect, s);
        s->setCellMaterial(mat);
        response_.push_back(s);
    }

    // check if the element is broken
    isFailed();
}

PlasticBeam3D::PlasticBeam3D(int id, Particle* pi, Particle* pj,
    const BaseMaterial* mat, const BaseSection* sect,
    const SectionGridParameter &para):
    Beam3D(id, pi, pj, mat, sect),lobatto(para.sections)
{
    initiatePlasticBeam3D(mat, sect, para);
}

PlasticBeam3D::PlasticBeam3D(int id, Particle* pi, Particle* pj,
    const BaseMaterial* mat, const BaseSection* sect, const StdArray3d ref_node,
    const SectionGridParameter &para):
    Beam3D(id, pi, pj, mat, sect, ref_node), lobatto(para.sections)
{
    initiatePlasticBeam3D(mat, sect, para);
}

PlasticBeam3D::~PlasticBeam3D()
{
    // release reponse_ container
    for (int i = 0; i < response_.size(); i++)
    {
        delete response_[i];
        response_[i] = nullptr;
    }
    response_.clear();
    response_.shrink_to_fit();
}

inline void PlasticBeam3D::isFailed()
{
    for (int i = 0; i < response_.size(); i++)
    {
        // degree of damage of Section reaches 1.0
        if (fabs(response_[i]->damageDegree() - 1) < 1e-6)
        {
            is_failed_ = true;
        }
    }
}

void PlasticBeam3D::calcElementForce()
{
    // calcOrientVector();
    exa_ = omega_.row(0);
    eya_ = omega_.row(1);
    eza_ = omega_.row(2);

    // element displace vector
    Eigen::Matrix<double, 6, 1> uvwi, uvwj;
    for (int i = 0; i < 6; i++)
    {
        uvwi(i) = (*curr_posi_)[i] - (*prev_posi_)[i];
        uvwj(i) = (*curr_posj_)[i] - (*prev_posj_)[i];
    }

    // step 1: principle axial rotation, change r_gamma_
    rotatePrincipleAxial();

    // step 2: convert particle rotation to element coordinate system and
    //         virtual reverse motion
    Eigen::Vector3d phi1h, phi2h, gamma_t;
    gamma_t << gamma_.dot(exa_), gamma_.dot(eya_), gamma_.dot(eza_);
    phi1h = omega_ * uvwi.tail<3>() - gamma_t;
    phi2h = omega_ * uvwj.tail<3>() - gamma_t;
    Eigen::VectorXd deform(5);
    deform << (lt_ - la_), phi1h(1), phi1h(2), phi2h(1), phi2h(2);

    // step 3: element forces & moment increasement in principal coordinate
    //         system at time t, but interpret at time ta
    // integral points position
    for (int i = 0; i < integrals_points_; i++)
    {
        double x = lobatto.x(i) * la_ / 2.0 + la_ / 2.0;

        // section internal force, Gauss-Legendre integration
        // tranfer matrix: node displacement --> section strain
        Eigen::RowVectorXd transfer(5);
        double s = x / la_;
        transfer << 1, 6 * s - 4, 4 - 6 * s, 6 * s - 2, 2 - 6 * s;
        response_[i]->response(transfer, deform);
    }

    // Element internal force, Gauss-Lobatto integration
    vector<double> force;
    for (int i = 0; i < 5; i++)
    {
        vector<double> tmp;
        for (int j = 0; j < integrals_points_; j++)
        {
            tmp.push_back(response_[j]->force[i]);
        }
        force.push_back(lobatto.integrate1d(tmp, 0, la_));
    }

    // node i
    em1h_(1) = force[1];                // miy
    em1h_(2) = force[2];                // miz

    // node j
    ef2h_(0) = force[0];                // axial force
    em2h_(0) += material_->G() * (section_->Iyy() + section_->Izz())
                * phi2h(0) / la_ ;       // T
    em2h_(1) = force[3];                // mjy
    em2h_(2) = force[4];                // mjz

    // Element equilibrium
    ef1h_(0) = -ef2h_(0);
    em1h_(0) = -em2h_(0);
    ef2h_(1) = -(em1h_(2) + em2h_(2)) / la_;
    ef2h_(2) =  (em1h_(1) + em2h_(1)) / la_;
    ef1h_(1) = -ef2h_(1);
    ef1h_(2) = -ef2h_(2);

   // step 4: Element forces & moment in global coordinate and forward motion
    Eigen::Vector3d ef1, ef2, em1, em2;
    ef1 = r_gamma_ * omega_.transpose() * ef1h_;
    ef2 = r_gamma_ * omega_.transpose() * ef2h_;
    em1 = r_gamma_ * omega_.transpose() * em1h_;
    em2 = r_gamma_ * omega_.transpose() * em2h_;

    // step 5: update coordinate transfer matrix
    ey_ = r_gamma_ * eya_;
    ez_ = ex_.cross(ey_);
    omega_ << ex_, ey_, ez_;
    omega_.transposeInPlace();

    // step 6: set element internal force, FIXME: may not right
    ef1h_ = omega_ * ef1;
    em1h_ = omega_ * em1;
    ef2h_ = omega_ * ef2;
    em2h_ = omega_ * em2;
    emfi_ = {.fx=ef1h_(0), .sfy=ef1h_(1), .sfz=ef1h_(2),
             .mx=em1h_(0), .my=em1h_(1), .mz=em1h_(2)};
    emfj_ = {.fx=ef2h_(0), .sfy=ef2h_(1), .sfz=ef2h_(2),
             .mx=em2h_(0), .my=em2h_(1), .mz=em2h_(2)};

    // fill fi_ and fj_
    fi_.fill(0);
    fj_.fill(0);
    for (int i = 0; i < 3; i++)
    {
        fi_[i] = -ef1(i);
        fj_[i] = -ef2(i);
    }
    for (int i = 0; i < 3; i++)
    {
        fi_[i+3] = -em1(i);
        fj_[i+3] = -em2(i);
    }

    // check if the element is broken
    isFailed();
}
